//!Includes

#include "ControlRobotTask.h"

//!Global Functions


namespace tasks
{

/**
 * class ControlRobotTask
 *
 *
 * Autor:David Bevilaqua
 *
 *
 * DESCRIPTION:
 *
 */

//!Constructors/Destructors

/**
 * Empty Constructor
 */
ControlRobotTask::ControlRobotTask (int argc,char **argv)
{
m_argv=argv;
m_argc=argc;
initAttributes();
}

/**
 * Empty Destructor
 */
ControlRobotTask::~ControlRobotTask ( )
{
	printf("control caiu\n");
}

//!Public Accessor Methods



//!Public Methods
//
void ControlRobotTask::ConectWithRobot ( ){
	m_RobotConection.SetArClient(m_pClient);
	m_RobotConection.ConectWithRobot(m_argc,m_argv);


}

ArClientBase* ControlRobotTask::GetArClient ( )
{
return m_RobotConection.GetArClient();
}

void ControlRobotTask::SetArClient (ArClientBase *Client )
{
m_pClient=Client;
}

bool ControlRobotTask::GetAchieveTarget()
{
return m_bAchieveTarget;
}

void ControlRobotTask::SetAchieveTarget (bool val)
{
	m_bAchieveTarget=val;
	m_IBVScontrol.SetAchieveTarget(false);
}
//!Protected Methods



//!Private Methods
void ControlRobotTask::AlignAxis()
{
	if((m_TargetVector.back().GetDiffRightTarget()+m_TargetVector.back().GetDiffLeftTarget())/2<15&&(m_TargetVector.back().GetDiffRightTarget()+m_TargetVector.back().GetDiffLeftTarget())/2>5&&m_TargetVector.back().GetDiffOrientation()>5){
		m_bAlign=true;
		StopRobot();
		ArUtil::sleep(1000);
		printf("Tentando alinhar\n");
		if(m_TargetVector.back().GetDiffEstOrientation()>0)
		{
			//printf("caso 1:\n");
			printf("caso 1: thetha:%d\n",std::abs(m_TargetVector.back().GetDiffOrientation()));
			printf("caso 1: thethaSin:%f\n",std::sin((std::abs(m_TargetVector.back().GetDiffOrientation())*3.1415/180)));
			SetHeading (-90+m_TargetVector.back().GetDiffOrientation());
			ArUtil::sleep(5000);
			Move(std::sin((std::abs(m_TargetVector.back().GetDiffOrientation())*3.1415/180))*2000);
			//Move(400);
			ArUtil::sleep(5000);
			SetHeading (0+std::abs(m_TargetVector.back().GetDiffOrientation()));
			ArUtil::sleep(5000);
		}
		else if(m_TargetVector.back().GetDiffEstOrientation()<0)
		{
			//printf("caso 2:\n");
			printf("caso 2: thetha:%d\n",std::abs(m_TargetVector.back().GetDiffOrientation()));
			printf("caso 2: thethaSin:%f\n",std::sin((std::abs(m_TargetVector.back().GetDiffOrientation())*3.1415/180)));
			SetHeading (90-m_TargetVector.back().GetDiffOrientation());
			ArUtil::sleep(5000);
			Move(std::sin((std::abs(m_TargetVector.back().GetDiffOrientation())*3.1415/180))*2000);
			//Move(400);
			ArUtil::sleep(5000);
			SetHeading (0-std::abs(m_TargetVector.back().GetDiffOrientation()));
			ArUtil::sleep(5000);
		}
		else
		{
			printf("pala manolo\n");
		}


	}
}

void ControlRobotTask::SetAlign(bool val)
{
m_bAlign=val;
}

bool ControlRobotTask::GetAlign()
{
return m_bAlign;
}

void ControlRobotTask::SetVelWheelRight (int VelRight)
{

	ArNetPacket pac;
	char buf[5];
	sprintf(buf,"%d",VelRight);
	pac.strToBuf(buf);
	m_pClient->requestOnce("SetVelWheelRight",&pac);
	ArUtil::sleep(10);
}

void ControlRobotTask::SetVelWheelLeft (int VelLeft)
{
	ArNetPacket pac;
	char buf[5];
	sprintf(buf,"%d",VelLeft);
	pac.strToBuf(buf);
	m_pClient->requestOnce("SetVelWheelLeft",&pac);
	ArUtil::sleep(10);

}

void ControlRobotTask::SetLinearVel (int VelLinear)
{
	ArNetPacket pac;
	char buf[5];
	sprintf(buf,"%d",VelLinear);
	pac.strToBuf(buf);
	m_pClient->requestOnce("SetLinearVel",&pac);
	ArUtil::sleep(10);
}

void ControlRobotTask::SetRotVel (int VelRot )
{
	ArNetPacket pac;
	char buf[5];
	sprintf(buf,"%d",VelRot);
	pac.strToBuf(buf);
	m_pClient->requestOnce("SetRotVel",&pac);
	ArUtil::sleep(10);
}

void ControlRobotTask::SetHeading (int Heading)
{
	ArNetPacket pac;
	char buf[10];
	Heading=Heading+m_TargetVector.back().GetPoseRobot().GetOrientation();
	sprintf(buf,"%d",Heading);
	pac.strToBuf(buf);
	m_pClient->requestOnce("SetHeading",&pac);
	ArUtil::sleep(10);
}

void ControlRobotTask::Move (int distance)
{
	ArNetPacket pac;
	char buf[5];
	sprintf(buf,"%d",distance);
	pac.strToBuf(buf);
	m_pClient->requestOnce("Move",&pac);
	ArUtil::sleep(10);
}

void ControlRobotTask::StopRobot()
{
	ArNetPacket pac;
	m_pClient->requestOnce("StopRobot",&pac);
	ArUtil::sleep(100);
}

void ControlRobotTask::LostTarget(bool lost)
{
m_bLost=lost;
}

void ControlRobotTask::SetTargetVector(std::vector <algorithms::Target> TargetVector)
{
m_TargetVector=TargetVector;
}

void ControlRobotTask::Execute(int val)
{
if(val==IBVS)
{
	m_IBVScontrol.SetTargetVecotor(m_TargetVector);
	m_IBVScontrol.LostTarget(m_bLost);
	m_IBVScontrol.Execute();
	m_bAchieveTarget=m_IBVScontrol.GetAchieveTarget();
	if(!m_IBVScontrol.StopRobot())
	{
		if(m_iLastVelLinear!=m_IBVScontrol.GetVelLinear()||m_iLastVelRot!=m_IBVScontrol.GetVelRot())
		{
		SetLinearVel(m_IBVScontrol.GetVelLinear());
		SetRotVel(m_IBVScontrol.GetVelRot());
		m_iLastVelLinear=m_IBVScontrol.GetVelLinear();
		m_iLastVelRot=m_IBVScontrol.GetVelRot();
		}
	}
	else
	{
		StopRobot();
	}
	if(m_bAlign==false)
	{
	AlignAxis();
	}

}
else if(PBVS)
{

	m_PBVScontrol.SetTargetVecotor(m_TargetVector);
	m_PBVScontrol.LostTarget(m_bLost);
	m_PBVScontrol.Execute();
	m_bAchieveTarget=m_PBVScontrol.GetAchieveTarget();
	if(!m_PBVScontrol.StopRobot())
	{
		SetLinearVel(m_PBVScontrol.GetVelLinear());
		SetRotVel(m_PBVScontrol.GetVelRot());
	}
	else
	{
		StopRobot();
	}

}

	m_TargetVector.clear();

}


/**
 * Starts all the attributes of
 * the ControlRobotTask
 */

void ControlRobotTask::initAttributes()
{
m_bAchieveTarget=false;
m_bAlign=false;
}

}
